/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

#ifdef _OPENMP
#include "omp.h"
#endif

// Uncomment to trade flexibility for speed
//#define CONST_HIST_SIZE

// Uncomment to get some performance stats in stderr
//#define REPORT_TICKS

#ifdef CONST_HIST_SIZE
#define m_BinBit 5
#define m_ByteShift 3
#endif

typedef float DefHistType;
#define DefHistTypeMat CV_32F
#define HIST_INDEX(_pData) (((_pData)[0]>>m_ByteShift) + (((_pData)[1]>>(m_ByteShift))<<m_BinBit)+((pImgData[2]>>m_ByteShift)<<(m_BinBit*2)))

class DefHist {
public:
	CvMat*          m_pHist;
	DefHistType     m_HistVolume;
	DefHist(int BinNum = 0) {
		m_pHist = NULL;
		m_HistVolume = 0;
		Resize(BinNum);
	}

	~DefHist() {
		if (m_pHist) { cvReleaseMat(&m_pHist); }
	}

	void Resize(int BinNum) {
		if (m_pHist) { cvReleaseMat(&m_pHist); }
		if (BinNum > 0) {
			m_pHist = cvCreateMat(1, BinNum, DefHistTypeMat);
			cvZero(m_pHist);
		}
		m_HistVolume = 0;
	}

	void Update(DefHist* pH, float W) {
		/* Update histogram: */
		double  Vol, WM, WC;
		Vol = 0.5 * (m_HistVolume + pH->m_HistVolume);
		WM = Vol * (1 - W) / m_HistVolume;
		WC = Vol * (W) / pH->m_HistVolume;
		cvAddWeighted(m_pHist, WM, pH->m_pHist, WC, 0, m_pHist);
		m_HistVolume = (float)cvSum(m_pHist).val[0];
	}   /* Update histogram: */
};

class CvBlobTrackerOneMSFG: public CvBlobTrackerOne {
protected:
	int             m_BinNumTotal; /* protected only for parralel MSPF */
	CvSize          m_ObjSize;

	void ReAllocKernel(int  w, int h) {
		int     x, y;
		float   x0 = 0.5f * (w - 1);
		float   y0 = 0.5f * (h - 1);
		assert(w > 0);
		assert(h > 0);
		m_ObjSize = cvSize(w, h);

		if (m_KernelHist) { cvReleaseMat(&m_KernelHist); }
		if (m_KernelMeanShift) { cvReleaseMat(&m_KernelMeanShift); }
		m_KernelHist = cvCreateMat(h, w, DefHistTypeMat);
		m_KernelMeanShift = cvCreateMat(h, w, DefHistTypeMat);

		for (y = 0; y < h; ++y) for (x = 0; x < w; ++x) {
				double r2 = ((x - x0) * (x - x0) / (x0 * x0) + (y - y0) * (y - y0) / (y0 * y0));
//            double r2 = ((x-x0)*(x-x0)+(y-y0)*(y-y0))/((y0*y0)+(x0*x0));
				CV_MAT_ELEM(m_KernelHist[0], DefHistType, y, x) = (DefHistType)GetKernelHist(r2);
				CV_MAT_ELEM(m_KernelMeanShift[0], DefHistType, y, x) = (DefHistType)GetKernelMeanShift(r2);
			}
	}

private:
	/* Parameters: */
	int             m_IterNum;
	float           m_FGWeight;
	float           m_Alpha;
	CvMat*          m_KernelHist;
	CvMat*          m_KernelMeanShift;
#ifndef CONST_HIST_SIZE
	int             m_BinBit;
	int             m_ByteShift;
#endif
	int             m_BinNum;
	int             m_Dim;
	/*
	CvMat*          m_HistModel;
	float           m_HistModelVolume;
	CvMat*          m_HistCandidate;
	float           m_HistCandidateVolume;
	CvMat*          m_HistTemp;
	*/
	DefHist         m_HistModel;
	DefHist         m_HistCandidate;
	DefHist         m_HistTemp;

	CvBlob          m_Blob;
	int             m_Collision;

	void ReAllocHist(int Dim, int BinBit) {
#ifndef CONST_HIST_SIZE
		m_BinBit = BinBit;
		m_ByteShift = 8 - BinBit;
#endif
		m_Dim = Dim;
		m_BinNum = (1 << BinBit);
		m_BinNumTotal = cvRound(pow((double)m_BinNum, (double)m_Dim));
		/*
		if(m_HistModel) cvReleaseMat(&m_HistModel);
		if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
		if(m_HistTemp) cvReleaseMat(&m_HistTemp);
		m_HistCandidate = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
		m_HistModel = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
		m_HistTemp = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
		cvZero(m_HistCandidate);
		cvZero(m_HistModel);
		m_HistModelVolume = 0.0f;
		m_HistCandidateVolume = 0.0f;
		*/
		m_HistCandidate.Resize(m_BinNumTotal);
		m_HistModel.Resize(m_BinNumTotal);
		m_HistTemp.Resize(m_BinNumTotal);
	}

	double GetKernelHist(double r2) {
		return (r2 < 1) ? 1 -  r2 : 0;
	}

	double GetKernelMeanShift(double r2) {
		return (r2 < 1) ? 1 : 0;
	}

//    void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pHist, DefHistType* pHistVolume)
//    void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, DefHist* pHist)

	void CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist) {
		int UsePrecalculatedKernel = 0;
		int BW = cvRound(pBlob->w);
		int BH = cvRound(pBlob->h);
		DefHistType Volume = 0;
		int         x0 = cvRound(pBlob->x - BW * 0.5);
		int         y0 = cvRound(pBlob->y - BH * 0.5);
		int         x, y;

		UsePrecalculatedKernel = (BW == m_ObjSize.width && BH == m_ObjSize.height ) ;

		//cvZero(pHist);
		cvSet(pHist->m_pHist, cvScalar(1.0 / m_BinNumTotal)); /* no zero bins, all bins have very small value*/
		Volume = 1;

		assert(BW < pImg->width);
		assert(BH < pImg->height);
		if ((x0 + BW) >= pImg->width) { BW = pImg->width - x0 - 1; }
		if ((y0 + BH) >= pImg->height) { BH = pImg->height - y0 - 1; }
		if (x0 < 0) { x0 = 0;}
		if (y0 < 0) { y0 = 0;}

		if (m_Dim == 3) {
			for (y = 0; y < BH; ++y) {
				unsigned char* pImgData = &CV_IMAGE_ELEM(pImg, unsigned char, y + y0, x0 * 3);
				unsigned char* pMaskData = pMask ? (&CV_IMAGE_ELEM(pMask, unsigned char, y + y0, x0)) : NULL;
				DefHistType* pKernelData = NULL;

				if (UsePrecalculatedKernel) {
					pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0], y, 0, sizeof(DefHistType)));
				}

				for (x = 0; x < BW; ++x, pImgData += 3) {
					DefHistType K;
					int index = HIST_INDEX(pImgData);
					assert(index >= 0 && index < pHist->m_pHist->cols);

					if (UsePrecalculatedKernel) {
						K = pKernelData[x];
					} else {
						float dx = (x + x0 - pBlob->x) / (pBlob->w * 0.5f);
						float dy = (y + y0 - pBlob->y) / (pBlob->h * 0.5f);
						double r2 = dx * dx + dy * dy;
						K = (float)GetKernelHist(r2);
					}

					if (pMaskData) {
						K *= pMaskData[x] * 0.003921568627450980392156862745098f;
					}
					Volume += K;
					((DefHistType*)(pHist->m_pHist->data.ptr))[index] += K;

				}   /* Next column. */
			}   /*  Next row. */
		}   /* if m_Dim == 3. */

		pHist->m_HistVolume = Volume;

	};  /* CollectHist */

	double calcBhattacharyya(DefHist* pHM = NULL, DefHist* pHC = NULL, DefHist* pHT = NULL) {
		if (pHM == NULL) { pHM = &m_HistModel; }
		if (pHC == NULL) { pHC = &m_HistCandidate; }
		if (pHT == NULL) { pHT = &m_HistTemp; }
		if (pHC->m_HistVolume * pHM->m_HistVolume > 0) {
#if 0
			// Use CV functions:
			cvMul(pHM->m_pHist, pHC->m_pHist, pHT->m_pHist);
			cvPow(pHT->m_pHist, pHT->m_pHist, 0.5);
			return cvSum(pHT->m_pHist).val[0] / sqrt(pHC->m_HistVolume * pHM->m_HistVolume);
#else
			// Do computations manually and let autovectorizer do the job:
			DefHistType* hm, *hc, *ht;
			double sum;
			int     size;
			hm = (DefHistType*)(pHM->m_pHist->data.ptr);
			hc = (DefHistType*)(pHC->m_pHist->data.ptr);
			ht = (DefHistType*)(pHT->m_pHist->data.ptr);
			size = pHM->m_pHist->width * pHM->m_pHist->height;
			sum = 0.;
			for (int i = 0; i < size; i++ ) {
				sum += sqrt(hm[i] * hc[i]);
			}
			return sum / sqrt(pHC->m_HistVolume * pHM->m_HistVolume);
#endif
		}
		return 0;
	}   /* calcBhattacharyyaCoefficient. */

protected:
	// double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, float x, float y, DefHist* pHist=NULL)
	double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob, DefHist* pHist = NULL, int /*thread_number*/ = 0) {
		if (pHist == NULL) { pHist = &m_HistTemp; }
		CollectHist(pImg, pImgFG, pBlob, pHist);
		return calcBhattacharyya(&m_HistModel, pHist, pHist);
	}

	void UpdateModelHist(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob) {
		if (m_Alpha > 0 && !m_Collision) {
			/* Update histogram: */
			CollectHist(pImg, pImgFG, pBlob, &m_HistCandidate);
			m_HistModel.Update(&m_HistCandidate, m_Alpha);
		}   /* Update histogram. */

	}   /* UpdateModelHist */

public:
	CvBlobTrackerOneMSFG() {

		/* Add several parameters for external use: */
		m_FGWeight = 2;
		AddParam("FGWeight", &m_FGWeight);
		CommentParam("FGWeight", "Weight of FG mask using (0 - mask will not be used for tracking)");

		m_Alpha = 0.01f;
		AddParam("Alpha", &m_Alpha);
		CommentParam("Alpha", "Coefficient for model histogram updating (0 - hist is not upated)");

		m_IterNum = 10;
		AddParam("IterNum", &m_IterNum);
		CommentParam("IterNum", "Maximal number of iteration in meanshift operation");

		/* Initialize internal data: */
		m_Collision = 0;
//        m_BinBit=0;
		m_Dim = 0;
		/*
		m_HistModel = NULL;
		m_HistCandidate = NULL;
		m_HistTemp = NULL;
		*/
		m_KernelHist = NULL;
		m_KernelMeanShift = NULL;
		ReAllocHist(3, 5);  /* 3D hist, each dim has 2^5 bins*/

		SetModuleName("MSFG");
	}

	~CvBlobTrackerOneMSFG() {
		/*
		if(m_HistModel) cvReleaseMat(&m_HistModel);
		if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
		if(m_HistTemp) cvReleaseMat(&m_HistTemp);
		*/
		if (m_KernelHist) { cvReleaseMat(&m_KernelHist); }
		if (m_KernelMeanShift) { cvReleaseMat(&m_KernelMeanShift); }
	}

	/* Interface: */
	virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL) {
		int w = cvRound(CV_BLOB_WX(pBlobInit));
		int h = cvRound(CV_BLOB_WY(pBlobInit));
		if (w < CV_BLOB_MINW) { w = CV_BLOB_MINW; }
		if (h < CV_BLOB_MINH) { h = CV_BLOB_MINH; }
		if (pImg) {
			if (w > pImg->width) { w = pImg->width; }
			if (h > pImg->height) { h = pImg->height; }
		}
		ReAllocKernel(w, h);
		if (pImg) {
			CollectHist(pImg, pImgFG, pBlobInit, &m_HistModel);
		}
		m_Blob = pBlobInit[0];
	};

	virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL) {
		int     iter;

		if (pBlobPrev) {
			m_Blob = pBlobPrev[0];
		}

		{
			/* Check blob size and realloc kernels if it is necessary: */
			int w = cvRound(m_Blob.w);
			int h = cvRound(m_Blob.h);
			if ( w != m_ObjSize.width || h != m_ObjSize.height) {
				ReAllocKernel(w, h);
				/* after this ( w != m_ObjSize.width || h!=m_ObjSize.height) shoiuld be false */
			}
		}   /* Check blob size and realloc kernels if it is necessary: */


		for (iter = 0; iter < m_IterNum; ++iter) {
			float   newx = 0, newy = 0, sum = 0;
			//int     x,y;
			double  B0;

			//CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
			CollectHist(pImg, NULL, &m_Blob, &m_HistCandidate);
			B0 = calcBhattacharyya();

			if (m_Wnd)if (CV_BLOB_ID(pBlobPrev) == 0 && iter == 0) {
					/* Debug: */
					IplImage*   pW = cvCloneImage(pImgFG);
					IplImage*   pWFG = cvCloneImage(pImgFG);
					int         x, y;

					cvZero(pW);
					cvZero(pWFG);

					assert(m_ObjSize.width < pImg->width);
					assert(m_ObjSize.height < pImg->height);

					/* Calculate shift vector: */
					for (y = 0; y < pImg->height; ++y) {
						unsigned char* pImgData = &CV_IMAGE_ELEM(pImg, unsigned char, y, 0);
						unsigned char* pMaskData = pImgFG ? (&CV_IMAGE_ELEM(pImgFG, unsigned char, y, 0)) : NULL;

						for (x = 0; x < pImg->width; ++x, pImgData += 3) {
							int         xk = cvRound(x - (m_Blob.x - m_Blob.w * 0.5));
							int         yk = cvRound(y - (m_Blob.y - m_Blob.h * 0.5));
							double      HM = 0;
							double      HC = 0;
							double      K;
							int index = HIST_INDEX(pImgData);
							assert(index >= 0 && index < m_BinNumTotal);

							if (fabs(x - m_Blob.x) > m_Blob.w * 0.6) { continue; }
							if (fabs(y - m_Blob.y) > m_Blob.h * 0.6) { continue; }

							if (xk < 0 || xk >= m_KernelMeanShift->cols) { continue; }
							if (yk < 0 || yk >= m_KernelMeanShift->rows) { continue; }

							if (m_HistModel.m_HistVolume > 0) {
								HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index] / m_HistModel.m_HistVolume;
							}

							if (m_HistCandidate.m_HistVolume > 0) {
								HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index] / m_HistCandidate.m_HistVolume;
							}

							K = *(DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0], yk, xk, sizeof(DefHistType));

							if (HC > 0) {
								double  V = sqrt(HM / HC);
								int     Vi = cvRound(V * 64);
								if (Vi < 0) { Vi = 0; }
								if (Vi > 255) { Vi = 255; }
								CV_IMAGE_ELEM(pW, uchar, y, x) = (uchar)Vi;

								V += m_FGWeight * (pMaskData ? (pMaskData[x] / 255.0f) : 0);
								V *= K;
								Vi = cvRound(V * 64);
								if (Vi < 0) { Vi = 0; }
								if (Vi > 255) { Vi = 255; }
								CV_IMAGE_ELEM(pWFG, uchar, y, x) = (uchar)Vi;
							}

						}   /* Next column. */
					}   /*  Next row. */

					//cvNamedWindow("MSFG_W",0);
					//cvShowImage("MSFG_W",pW);
					//cvNamedWindow("MSFG_WFG",0);
					//cvShowImage("MSFG_WFG",pWFG);
					//cvNamedWindow("MSFG_FG",0);
					//cvShowImage("MSFG_FG",pImgFG);

					//cvSaveImage("MSFG_W.bmp",pW);
					//cvSaveImage("MSFG_WFG.bmp",pWFG);
					//cvSaveImage("MSFG_FG.bmp",pImgFG);

				}   /* Debug. */


			/* Calculate new position by meanshift: */

			/* Calculate new position: */
			if (m_Dim == 3) {
				int         x0 = cvRound(m_Blob.x - m_ObjSize.width * 0.5);
				int         y0 = cvRound(m_Blob.y - m_ObjSize.height * 0.5);
				int         x, y;

				assert(m_ObjSize.width < pImg->width);
				assert(m_ObjSize.height < pImg->height);

				/* Crop blob bounds: */
				if ((x0 + m_ObjSize.width) >= pImg->width) { x0 = pImg->width - m_ObjSize.width - 1; }
				if ((y0 + m_ObjSize.height) >= pImg->height) { y0 = pImg->height - m_ObjSize.height - 1; }
				if (x0 < 0) { x0 = 0;}
				if (y0 < 0) { y0 = 0;}

				/* Calculate shift vector: */
				for (y = 0; y < m_ObjSize.height; ++y) {
					unsigned char* pImgData = &CV_IMAGE_ELEM(pImg, unsigned char, y + y0, x0 * 3);
					unsigned char* pMaskData = pImgFG ? (&CV_IMAGE_ELEM(pImgFG, unsigned char, y + y0, x0)) : NULL;
					DefHistType* pKernelData = (DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0], y, 0, sizeof(DefHistType));

					for (x = 0; x < m_ObjSize.width; ++x, pImgData += 3) {
						DefHistType K = pKernelData[x];
						double      HM = 0;
						double      HC = 0;
						int index = HIST_INDEX(pImgData);
						assert(index >= 0 && index < m_BinNumTotal);

						if (m_HistModel.m_HistVolume > 0) {
							HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index] / m_HistModel.m_HistVolume;
						}

						if (m_HistCandidate.m_HistVolume > 0) {
							HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index] / m_HistCandidate.m_HistVolume;
						}

						if (HC > 0) {
							double V = sqrt(HM / HC);
							if (!m_Collision && m_FGWeight > 0 && pMaskData) {
								V += m_FGWeight * (pMaskData[x] / 255.0f);
							}
							K *= (float)MIN(V, 100000.);
						}

						sum += K;
						newx += K * x;
						newy += K * y;
					}   /* Next column. */
				}   /*  Next row. */

				if (sum > 0) {
					newx /= sum;
					newy /= sum;
				}
				newx += x0;
				newy += y0;

			}   /* if m_Dim == 3. */

			/* Calculate new position by meanshift: */

			for (;;) {
				/* Iterate using bahattcharrya coefficient: */
				double  B1;
				CvBlob  B = m_Blob;
//                CvPoint NewCenter = cvPoint(cvRound(newx),cvRound(newy));
				B.x = newx;
				B.y = newy;
				CollectHist(pImg, NULL, &B, &m_HistCandidate);
				B1 = calcBhattacharyya();
				if (B1 > B0) { break; }
				newx = 0.5f * (newx + m_Blob.x);
				newy = 0.5f * (newy + m_Blob.y);
				if (fabs(newx - m_Blob.x) < 0.1 && fabs(newy - m_Blob.y) < 0.1) { break; }
			}   /* Iterate using bahattcharrya coefficient. */

			if (fabs(newx - m_Blob.x) < 0.5 && fabs(newy - m_Blob.y) < 0.5) { break; }
			m_Blob.x = newx;
			m_Blob.y = newy;
		}   /* Next iteration. */

		while (!m_Collision && m_FGWeight > 0) {
			/* Update size if no collision. */
			float       Alpha = 0.04f;
			CvBlob      NewBlob;
			double      M00, X, Y, XX, YY;
			CvMoments   m;
			CvRect      r;
			CvMat       mat;

			r.width = cvRound(m_Blob.w * 1.5 + 0.5);
			r.height = cvRound(m_Blob.h * 1.5 + 0.5);
			r.x = cvRound(m_Blob.x - 0.5 * r.width);
			r.y = cvRound(m_Blob.y - 0.5 * r.height);
			if (r.x < 0) { break; }
			if (r.y < 0) { break; }
			if (r.x + r.width >= pImgFG->width) { break; }
			if (r.y + r.height >= pImgFG->height) { break; }
			if (r.height < 5 || r.width < 5) { break; }

			cvMoments( cvGetSubRect(pImgFG, &mat, r), &m, 0 );
			M00 = cvGetSpatialMoment( &m, 0, 0 );
			if (M00 <= 0 ) { break; }
			X = cvGetSpatialMoment( &m, 1, 0 ) / M00;
			Y = cvGetSpatialMoment( &m, 0, 1 ) / M00;
			XX = (cvGetSpatialMoment( &m, 2, 0 ) / M00) - X * X;
			YY = (cvGetSpatialMoment( &m, 0, 2 ) / M00) - Y * Y;
			NewBlob = cvBlob(r.x + (float)X, r.y + (float)Y, (float)(4 * sqrt(XX)), (float)(4 * sqrt(YY)));

			NewBlob.w = Alpha * NewBlob.w + m_Blob.w * (1 - Alpha);
			NewBlob.h = Alpha * NewBlob.h + m_Blob.h * (1 - Alpha);

			m_Blob.w = MAX(NewBlob.w, 5);
			m_Blob.h = MAX(NewBlob.h, 5);
			break;

		}   /* Update size if no collision. */

		return &m_Blob;

	};  /* CvBlobTrackerOneMSFG::Process */

	virtual double GetConfidence(CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL, IplImage* pImgUnusedReg = NULL) {
		double  S = 0.2;
		double  B = GetBhattacharyya(pImg, pImgUnusedReg, pBlob, &m_HistTemp);
		return exp((B - 1) / (2 * S));

	};  /*CvBlobTrackerOneMSFG::*/

	virtual void Update(CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG = NULL) {
		/* Update histogram: */
		UpdateModelHist(pImg, pImgFG, pBlob ? pBlob : &m_Blob);
	}   /*CvBlobTrackerOneMSFG::*/

	virtual void Release() {delete this;};
	virtual void SetCollision(int CollisionFlag) {
		m_Collision = CollisionFlag;
	}
	virtual void SaveState(CvFileStorage* fs) {
		cvWriteStruct(fs, "Blob", &m_Blob, "ffffi");
		cvWriteInt(fs, "Collision", m_Collision);
		cvWriteInt(fs, "HistVolume", cvRound(m_HistModel.m_HistVolume));
		cvWrite(fs, "Hist", m_HistModel.m_pHist);
	};
	virtual void LoadState(CvFileStorage* fs, CvFileNode* node) {
		CvMat* pM;
		cvReadStructByName(fs, node, "Blob", &m_Blob, "ffffi");
		m_Collision = cvReadIntByName(fs, node, "Collision", m_Collision);
		pM = (CvMat*)cvRead(fs, cvGetFileNodeByName(fs, node, "Hist"));
		if (pM) {
			m_HistModel.m_pHist = pM;
			m_HistModel.m_HistVolume = (float)cvSum(pM).val[0];
		}
	};

};  /*CvBlobTrackerOneMSFG*/

#if 0
void CvBlobTrackerOneMSFG::CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist) {
	int UsePrecalculatedKernel = 0;
	int BW = cvRound(pBlob->w);
	int BH = cvRound(pBlob->h);
	DefHistType Volume = 0;
	int         x0 = cvRound(pBlob->x - BW * 0.5);
	int         y0 = cvRound(pBlob->y - BH * 0.5);
	int         x, y;

	UsePrecalculatedKernel = (BW == m_ObjSize.width && BH == m_ObjSize.height ) ;

	//cvZero(pHist);
	cvSet(pHist->m_pHist, cvScalar(1.0 / m_BinNumTotal)); /* no zero bins, all bins have very small value*/
	Volume = 1;

	assert(BW < pImg->width);
	assert(BH < pImg->height);
	if ((x0 + BW) >= pImg->width) { BW = pImg->width - x0 - 1; }
	if ((y0 + BH) >= pImg->height) { BH = pImg->height - y0 - 1; }
	if (x0 < 0) { x0 = 0;}
	if (y0 < 0) { y0 = 0;}

	if (m_Dim == 3) {
		for (y = 0; y < BH; ++y) {
			unsigned char* pImgData = &CV_IMAGE_ELEM(pImg, unsigned char, y + y0, x0 * 3);
			unsigned char* pMaskData = pMask ? (&CV_IMAGE_ELEM(pMask, unsigned char, y + y0, x0)) : NULL;
			DefHistType* pKernelData = NULL;

			if (UsePrecalculatedKernel) {
				pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0], y, 0, sizeof(DefHistType)));
			}

			for (x = 0; x < BW; ++x, pImgData += 3) {
				DefHistType K;
				int index = HIST_INDEX(pImgData);
				assert(index >= 0 && index < pHist->m_pHist->cols);

				if (UsePrecalculatedKernel) {
					K = pKernelData[x];
				} else {
					float dx = (x + x0 - pBlob->x) / (pBlob->w * 0.5);
					float dy = (y + y0 - pBlob->y) / (pBlob->h * 0.5);
					double r2 = dx * dx + dy * dy;
					K = GetKernelHist(r2);
				}

				if (pMaskData) {
					K *= pMaskData[x] * 0.003921568627450980392156862745098;
				}
				Volume += K;
				((DefHistType*)(pHist->m_pHist->data.ptr))[index] += K;

			}   /* Next column. */
		}   /*  Next row. */
	}   /*  if m_Dim == 3. */

	pHist->m_HistVolume = Volume;

};  /* CollectHist */
#endif

CvBlobTrackerOne* cvCreateBlobTrackerOneMSFG() {
	return (CvBlobTrackerOne*) new CvBlobTrackerOneMSFG;
}

CvBlobTracker* cvCreateBlobTrackerMSFG() {
	return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSFG);
}

/* Create specific tracker without FG
 * usin - just simple mean-shift tracker: */
class CvBlobTrackerOneMS: public CvBlobTrackerOneMSFG {
public:
	CvBlobTrackerOneMS() {
		SetParam("FGWeight", 0);
		DelParam("FGWeight");
		SetModuleName("MS");
	};
};

CvBlobTrackerOne* cvCreateBlobTrackerOneMS() {
	return (CvBlobTrackerOne*) new CvBlobTrackerOneMS;
}

CvBlobTracker* cvCreateBlobTrackerMS() {
	return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMS);
}

typedef struct DefParticle {
	CvBlob  blob;
	float   Vx, Vy;
	double  W;
} DefParticle;

class CvBlobTrackerOneMSPF: public CvBlobTrackerOneMS {
private:
	/* parameters */
	int             m_ParticleNum;
	float           m_UseVel;
	float           m_SizeVar;
	float           m_PosVar;

	CvSize          m_ImgSize;
	CvBlob          m_Blob;
	DefParticle*    m_pParticlesPredicted;
	DefParticle*    m_pParticlesResampled;
	CvRNG           m_RNG;
#ifdef _OPENMP
	int             m_ThreadNum;
	DefHist*        m_HistForParalel;
#endif

public:
	virtual void SaveState(CvFileStorage* fs) {
		CvBlobTrackerOneMS::SaveState(fs);
		cvWriteInt(fs, "ParticleNum", m_ParticleNum);
		cvWriteStruct(fs, "ParticlesPredicted", m_pParticlesPredicted, "ffffiffd", m_ParticleNum);
		cvWriteStruct(fs, "ParticlesResampled", m_pParticlesResampled, "ffffiffd", m_ParticleNum);
	};

	virtual void LoadState(CvFileStorage* fs, CvFileNode* node) {
		//CvMat* pM;
		CvBlobTrackerOneMS::LoadState(fs, node);
		m_ParticleNum = cvReadIntByName(fs, node, "ParticleNum", m_ParticleNum);
		if (m_ParticleNum > 0) {
			Realloc();
			printf("sizeof(DefParticle) is %d\n", (int)sizeof(DefParticle));
			cvReadStructByName(fs, node, "ParticlesPredicted", m_pParticlesPredicted, "ffffiffd");
			cvReadStructByName(fs, node, "ParticlesResampled", m_pParticlesResampled, "ffffiffd");
		}
	};
	CvBlobTrackerOneMSPF() {
		m_pParticlesPredicted = NULL;
		m_pParticlesResampled = NULL;
		m_ParticleNum = 200;

		AddParam("ParticleNum", &m_ParticleNum);
		CommentParam("ParticleNum", "Number of particles");
		Realloc();

		m_UseVel = 0;
		AddParam("UseVel", &m_UseVel);
		CommentParam("UseVel", "Percent of particles which use velocity feature");

		m_SizeVar = 0.05f;
		AddParam("SizeVar", &m_SizeVar);
		CommentParam("SizeVar", "Size variation (in object size)");

		m_PosVar = 0.2f;
		AddParam("PosVar", &m_PosVar);
		CommentParam("PosVar", "Position variation (in object size)");

		m_RNG = cvRNG(0);

		SetModuleName("MSPF");

#ifdef _OPENMP
		{
			m_ThreadNum = omp_get_num_procs();
			m_HistForParalel = new DefHist[m_ThreadNum];
		}
#endif
	}

	~CvBlobTrackerOneMSPF() {
		if (m_pParticlesResampled) { cvFree(&m_pParticlesResampled); }
		if (m_pParticlesPredicted) { cvFree(&m_pParticlesPredicted); }
#ifdef _OPENMP
		if (m_HistForParalel) { delete[] m_HistForParalel; }
#endif
	}

private:
	void Realloc() {
		if (m_pParticlesResampled) { cvFree(&m_pParticlesResampled); }
		if (m_pParticlesPredicted) { cvFree(&m_pParticlesPredicted); }
		m_pParticlesPredicted = (DefParticle*)cvAlloc(sizeof(DefParticle) * m_ParticleNum);
		m_pParticlesResampled = (DefParticle*)cvAlloc(sizeof(DefParticle) * m_ParticleNum);
	};  /* Realloc*/

	void DrawDebug(IplImage* pImg, IplImage* /*pImgFG*/) {
		int k;
		for (k = 0; k < 2; ++k) {
			DefParticle*    pBP = k ? m_pParticlesResampled : m_pParticlesPredicted;
			//const char*   name = k?"MSPF resampled particle":"MSPF Predicted particle";
			IplImage*       pI = cvCloneImage(pImg);
			int             h, hN = m_ParticleNum;
			CvBlob          C = cvBlob(0, 0, 0, 0);
			double          WS = 0;
			for (h = 0; h < hN; ++h) {
				CvBlob  B = pBP[h].blob;
				int     CW = cvRound(255 * pBP[h].W);
				CvBlob* pB = &B;
				int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
				CvSize  s = cvSize(MAX(1, x), MAX(1, y));
				double  W = pBP[h].W;
				C.x += pB->x;
				C.y += pB->y;
				C.w += pB->w;
				C.h += pB->h;
				WS += W;

				s = cvSize(1, 1);
				cvEllipse( pI,
						   cvPointFrom32f(CV_BLOB_CENTER(pB)),
						   s,
						   0, 0, 360,
						   CV_RGB(CW, 0, 0), 1 );

			}   /* Next hypothesis. */

			C.x /= hN;
			C.y /= hN;
			C.w /= hN;
			C.h /= hN;

			cvEllipse( pI,
					   cvPointFrom32f(CV_BLOB_CENTER(&C)),
					   cvSize(cvRound(C.w * 0.5), cvRound(C.h * 0.5)),
					   0, 0, 360,
					   CV_RGB(0, 0, 255), 1 );

			cvEllipse( pI,
					   cvPointFrom32f(CV_BLOB_CENTER(&m_Blob)),
					   cvSize(cvRound(m_Blob.w * 0.5), cvRound(m_Blob.h * 0.5)),
					   0, 0, 360,
					   CV_RGB(0, 255, 0), 1 );

			//cvNamedWindow(name,0);
			//cvShowImage(name,pI);
			cvReleaseImage(&pI);
		} /*  */

		//printf("Blob %d, point (%.1f,%.1f) size (%.1f,%.1f)\n",m_Blob.ID,m_Blob.x,m_Blob.y,m_Blob.w,m_Blob.h);
	} /* ::DrawDebug */

private:
	void Prediction() {
		int p;
		for (p = 0; p < m_ParticleNum; ++p) {
			/* "Prediction" of particle: */
			//double  t;
			float   r[5];
			CvMat   rm = cvMat(1, 5, CV_32F, r);
			cvRandArr(&m_RNG, &rm, CV_RAND_NORMAL, cvScalar(0), cvScalar(1));

			m_pParticlesPredicted[p] = m_pParticlesResampled[p];

			if (cvRandReal(&m_RNG) < 0.5) {
				/* Half of particles will predict based on external blob: */
				m_pParticlesPredicted[p].blob = m_Blob;
			}

			if (cvRandReal(&m_RNG) < m_UseVel) {
				/* Predict moving particle by usual way by using speed: */
				m_pParticlesPredicted[p].blob.x += m_pParticlesPredicted[p].Vx;
				m_pParticlesPredicted[p].blob.y += m_pParticlesPredicted[p].Vy;
			} else {
				/* Stop several particles: */
				m_pParticlesPredicted[p].Vx = 0;
				m_pParticlesPredicted[p].Vy = 0;
			}

			{
				/* Update position: */
				float S = (m_Blob.w + m_Blob.h) * 0.5f;
				m_pParticlesPredicted[p].blob.x += m_PosVar * S * r[0];
				m_pParticlesPredicted[p].blob.y += m_PosVar * S * r[1];

				/* Update velocity: */
				m_pParticlesPredicted[p].Vx += (float)(m_PosVar * S * 0.1 * r[3]);
				m_pParticlesPredicted[p].Vy += (float)(m_PosVar * S * 0.1 * r[4]);
			}

			/* Update size: */
			m_pParticlesPredicted[p].blob.w *= (1 + m_SizeVar * r[2]);
			m_pParticlesPredicted[p].blob.h *= (1 + m_SizeVar * r[2]);

			/* Truncate size of particle: */
			if (m_pParticlesPredicted[p].blob.w > m_ImgSize.width * 0.5f) {
				m_pParticlesPredicted[p].blob.w = m_ImgSize.width * 0.5f;
			}

			if (m_pParticlesPredicted[p].blob.h > m_ImgSize.height * 0.5f) {
				m_pParticlesPredicted[p].blob.h = m_ImgSize.height * 0.5f;
			}

			if (m_pParticlesPredicted[p].blob.w < 1 ) {
				m_pParticlesPredicted[p].blob.w = 1;
			}

			if (m_pParticlesPredicted[p].blob.h < 1) {
				m_pParticlesPredicted[p].blob.h = 1;
			}
		}   /* "Prediction" of particle. */
	}   /* Prediction */

	void UpdateWeightsMS(IplImage* pImg, IplImage* /*pImgFG*/) {
		int p;
#ifdef _OPENMP
		if ( m_HistForParalel[0].m_pHist == NULL || m_HistForParalel[0].m_pHist->cols != m_BinNumTotal) {
			int t;
			for (t = 0; t < m_ThreadNum; ++t) {
				m_HistForParalel[t].Resize(m_BinNumTotal);
			}
		}
#endif

#ifdef _OPENMP
#pragma omp parallel for num_threads(m_ThreadNum) schedule(runtime)
#endif
		for (p = 0; p < m_ParticleNum; ++p) {
			/* Calculate weights for particles: */
			double  S = 0.2;
			double  B = 0;
#ifdef _OPENMP
			assert(omp_get_thread_num() < m_ThreadNum);
#endif

			B = GetBhattacharyya(
					pImg, NULL,
					&(m_pParticlesPredicted[p].blob)
#ifdef _OPENMP
					, &(m_HistForParalel[omp_get_thread_num()])
#endif
				);
			m_pParticlesPredicted[p].W *= exp((B - 1) / (2 * S));

		}   /* Calculate weights for particles. */
	}

	void UpdateWeightsCC(IplImage* /*pImg*/, IplImage* /*pImgFG*/) {
		int p;
#ifdef _OPENMP
#pragma omp parallel for
#endif
		for (p = 0; p < m_ParticleNum; ++p) {
			/* Calculate weights for particles: */
			double W = 1;
			m_pParticlesPredicted[p].W *= W;
		}   /* Calculate weights for particles. */
	}

	void Resample() {
		/* Resample particle: */
		int         p;
		double      Sum = 0;

		for (p = 0; p < m_ParticleNum; ++p) {
			Sum += m_pParticlesPredicted[p].W;
		}

		for (p = 0; p < m_ParticleNum; ++p) {
			double  T = Sum * cvRandReal(&m_RNG);   /* Set current random threshold for cululative weight. */
			int     p2;
			double  Sum2 = 0;

			for (p2 = 0; p2 < m_ParticleNum; ++p2) {
				Sum2 += m_pParticlesPredicted[p2].W;
				if (Sum2 >= T) { break; }
			}

			if (p2 >= m_ParticleNum) { p2 = m_ParticleNum - 1; }
			m_pParticlesResampled[p] = m_pParticlesPredicted[p2];
			m_pParticlesResampled[p].W = 1;

		}   /* Find next particle. */
	}   /*  Resample particle. */


public:
	virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL) {
		int i;
		CvBlobTrackerOneMSFG::Init(pBlobInit, pImg, pImgFG);
		DefParticle PP;
		PP.W = 1;
		PP.Vx = 0;
		PP.Vy = 0;
		PP.blob = pBlobInit[0];
		for (i = 0; i < m_ParticleNum; ++i) {
			m_pParticlesPredicted[i] = PP;
			m_pParticlesResampled[i] = PP;
		}
		m_Blob = pBlobInit[0];

	}   /* CvBlobTrackerOneMSPF::Init*/

	virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL) {
		int p;

		m_ImgSize.width = pImg->width;
		m_ImgSize.height = pImg->height;


		m_Blob = pBlobPrev[0];

		{
			/* Check blob size and realloc kernels if it is necessary: */
			int w = cvRound(m_Blob.w);
			int h = cvRound(m_Blob.h);
			if ( w != m_ObjSize.width || h != m_ObjSize.height) {
				ReAllocKernel(w, h);
				/* After this ( w != m_ObjSize.width || h!=m_ObjSize.height) should be false. */
			}
		}   /* Check blob size and realloc kernels if it is necessary. */

		Prediction();

#ifdef REPORT_TICKS
		int64 ticks = cvGetTickCount();
#endif

		UpdateWeightsMS(pImg, pImgFG);

#ifdef REPORT_TICKS
		ticks = cvGetTickCount() - ticks;
		fprintf(stderr, "PF UpdateWeights, %d ticks\n",  (int)ticks);
		ticks = cvGetTickCount();
#endif

		Resample();

#ifdef REPORT_TICKS
		ticks = cvGetTickCount() - ticks;
		fprintf(stderr, "PF Resampling, %d ticks\n",  (int)ticks);
#endif

		{
			/* Find average result: */
			float   x = 0;
			float   y = 0;
			float   w = 0;
			float   h = 0;
			float   Sum = 0;

			DefParticle* pP = m_pParticlesResampled;

			for (p = 0; p < m_ParticleNum; ++p) {
				float W = (float)pP[p].W;
				x += W * pP[p].blob.x;
				y += W * pP[p].blob.y;
				w += W * pP[p].blob.w;
				h += W * pP[p].blob.h;
				Sum += W;
			}

			if (Sum > 0) {
				m_Blob.x = x / Sum;
				m_Blob.y = y / Sum;
				m_Blob.w = w / Sum;
				m_Blob.h = h / Sum;
			}
		}   /* Find average result. */

		if (m_Wnd) {
			DrawDebug(pImg, pImgFG);
		}

		return &m_Blob;

	}   /* CvBlobTrackerOneMSPF::Process */

	virtual void SkipProcess(CvBlob* pBlob, IplImage* /*pImg*/, IplImage* /*pImgFG*/ = NULL) {
		int p;
		for (p = 0; p < m_ParticleNum; ++p) {
			m_pParticlesResampled[p].blob = pBlob[0];
			m_pParticlesResampled[p].Vx = 0;
			m_pParticlesResampled[p].Vy = 0;
			m_pParticlesResampled[p].W = 1;
		}
	}

	virtual void Release() {delete this;};
	virtual void ParamUpdate() {
		Realloc();
	}

};  /* CvBlobTrackerOneMSPF */

CvBlobTrackerOne* cvCreateBlobTrackerOneMSPF() {
	return (CvBlobTrackerOne*) new CvBlobTrackerOneMSPF;
}

CvBlobTracker* cvCreateBlobTrackerMSPF() {
	return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSPF);
}

